import os
import time
import matplotlib.pyplot as plt
import numpy as np
from math import radians
from numpy import genfromtxt

plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False

x_length = 8000
y_length = 25*0.2*1000 #y_length由电机运行时间决定，电机运行速度固定0.2m/s,此处电机运行25s
z_length = 8000
cmap_kinds = "jet"

global val

# 求身高
def height(hx,hy,hz):


    x_d_max = -99999
    x_d_min = 99999
    z_h_max = -99999
    z_h_min = 99999
    y_w_max = -99999
    y_w_min = 99999

    # 空间调整，限制测量范围
    xtmin = -600
    xtmax = 200
    ytmin = 2500
    ytmax = 3500
    ztmin = -3000
    ztmax = -500

    condition = (hx > xtmin) & (hx < xtmax) & (hy > ytmin) & (hy < ytmax) & (hz > ztmin) & (hz < ztmax)
    x = hx[condition]
    y = hy[condition]
    z = hz[condition]

    for i in range(0, len(z)):
        if z[i] > z_h_max:
            z_h_max = z[i]
    for i in range(0, len(z)):
        if z[i] < z_h_min:
            z_h_min = z[i]
    print("z最小值：" + str(z_h_min))
    print("z最大值：" + str(z_h_max))
    people_h = round(((z_h_max - z_h_min) / 10)*0.985, 1)
    if people_h<70:
        print("当前测量区域无人，请前往测量区域进行测量身高")
        return people_h, x, y, z
    else:
        print(f"身高估算值：{people_h}厘米")
        return people_h,x,y,z

def work():
    # 读取文件夹
    files = os.listdir(r'C:\Users\赵浩中\PycharmProjects\pythonProject1\data\box')
    filename = None

    global saveFig
    saveFig = 1
    # 读取文件名称
    for file in files:
        if file.split('.')[0] in ['lidar-10231647']:
            filename = os.path.splitext(file)[0]
            print("文件名称：" + filename + ".csv")

            global file_path
            # file_path = r'M:\Desktop' + '\\' + filename + '.csv'
            file_path = r'C:\Users\赵浩中\PycharmProjects\pythonProject1\data\box\12-04 17.22.csv'
            global filesize
            filesize = os.path.getsize(file_path)
            print(f"文件大小： {round(filesize / 1000000, 2)}MB")
            print(f"数据处理中.....")
            while filesize > 1000000:
                time.sleep(5)
                d100 = genfromtxt(file_path, delimiter=',')

                theta = 0
                pi = 3.1415926
                x = -d100[:, 2]  # * np.cos(theta / 180 * pi) + d100[:, 2] * np.sin(theta / 180 * pi)
                z = -d100[:, 1]  # * np.cos(theta / 180 * pi) - d100[:, 1] * np.sin(theta / 180 * pi)
                # y = [i/len(x)*y_length for i in range(len(x))]
                y = d100[:, 3]
                # y = [i * 1.025 for i in d100[:, 3]]
                # y = np.array(y)
                
                # 地面 R(x > 0) & (x < 2500) & (y > 2050) & (y < 2400) L(x > -3000) & (x < 0) & (y > 2000) & (y < 2250) A(x > -3000) & (x < 2800) & (y > 2000) & (y < 2250)
                condition = (x > -2000) & (x < 2000) & (y > 2000) & (y < 2250) & (z < -2600)
                # condition = (x > -1000) & (x < 0) & (y > 3000) & (y < 4000) & (z < -2500)
                x_floor = x[condition]
                y_floor = y[condition]
                z_floor = z[condition]

                def adjust_roll(roll_angle, x, y, z):
                    pi = 3.1415926
                    x = x * np.cos(roll_angle / 180 * pi) + z * np.sin(roll_angle / 180 * pi)
                    y = y
                    z = z * np.cos(roll_angle / 180 * pi) - x * np.sin(roll_angle / 180 * pi)
                    return x, y, z

                def adjust_pitch(pitch_angle, x, y, z):
                    pi = 3.1415926
                    x = x
                    y = y * np.cos(pitch_angle / 180 * pi) + z * np.sin(pitch_angle / 180 * pi)
                    z = z * np.cos(pitch_angle / 180 * pi) - y * np.sin(pitch_angle / 180 * pi)
                    return x, y, z

                # roll
                nplot = 5
                maxangle = int(10)
                # fig = plt.figure(figsize=(16, 16))
                min_std = 999999
                roll_angle = 0

                for i in range(-maxangle, maxangle + 1, 1):

                    theta1 = i
                    pi = 3.1415926
                    x1 = x_floor * np.cos(theta1 / 180 * pi) + z_floor * np.sin(theta1 / 180 * pi)
                    y1 = y_floor
                    z1 = z_floor * np.cos(theta1 / 180 * pi) - x_floor * np.sin(theta1 / 180 * pi)

                    # ax = fig.add_subplot(nplot, nplot, i + maxangle + 1)
                    # ax.scatter(x1, z1, s=1, color='b')
                    i_std = np.std(z1)
                    # print("角度："+str(i)+"    标准差"+str(i_std))
                    # ax.set_title('%i - %f' % (i, i_std))
                    if i_std < min_std:
                        min_std = i_std
                        roll_angle = i
                #     ax.set_ylim([-3000, -2000])
                # plt.subplots_adjust(hspace=0.5)
                # plt.show()
                # print("最佳角度："+str(roll_angle))

                x_floor, y_floor, z_floor = adjust_roll(roll_angle, x_floor, y_floor, z_floor)

                # pitch
                nplot = 5
                maxangle = int(10)
                # fig = plt.figure(figsize=(16, 16))
                min_std = 999999
                pitch_angle = 0

                for i in range(-maxangle, maxangle + 1, 1):

                    theta2 = i
                    pi = 3.1415926
                    x2 = x_floor
                    y2 = y_floor * np.cos(theta2 / 180 * pi) + z_floor * np.sin(theta2 / 180 * pi)
                    z2 = z_floor * np.cos(theta2 / 180 * pi) - y_floor * np.sin(theta2 / 180 * pi)

                    # ax = fig.add_subplot(nplot, nplot, i + maxangle + 1)
                    # ax.scatter(y2, z2, s=1, color='b')
                    i_std = np.std(z2)
                    # print("角度："+str(i)+"    标准差"+str(i_std))
                    # ax.set_title('%i - %f' % (i, i_std))
                    if i_std < min_std:
                        min_std = i_std
                        pitch_angle = i
                #     ax.set_xlim([2000, 3000])
                #     ax.set_ylim([-3000, -2000])
                # plt.subplots_adjust(hspace=0.5)
                # plt.show()
                # print("最佳角度：" + str(pitch_angle))
                x_floor, y_floor, z_floor = adjust_pitch(pitch_angle, x_floor, y_floor, z_floor)

                # 校正数据
                x, y, z = adjust_roll(roll_angle, x, y, z)
                x, y, z = adjust_pitch(pitch_angle, x, y, z)
                # print(len(x))
                
                # condition_table = (x > -6000) & (x < 0) & (y > 3000) & (y < 4000) & (z < -1500) & (z > -2500)
                # x_table, y_table, z_table = x[condition_table], y[condition_table], z[condition_table]
                # fig = plt.figure()  
                # ax = fig.add_subplot(111)  
                # ax.scatter(x_table, y_table, s=0.1)  
                # ax.set_xlabel('X')  
                # ax.set_ylabel('z')  
                # plt.show()
                # exit()
                # fig = plt.figure()  
                # ax = fig.add_subplot(111, projection='3d')  
                # ax.scatter(x_table, y_table, z_table)  
                # ax.set_xlabel('X')  
                # ax.set_ylabel('Y')  
                # ax.set_zlabel('Z')
                # plt.show()
                # exit()

                # # 桌子
                # condition = (x > 250) & (x < 4250) & (y > 2750) & (y < 4250) & (z < -1700) & (z > -2200)
                # # condition = (x > -1000) & (x < 0) & (y > 3000) & (y < 4000) & (z < -2500)
                # x_table = x[condition]
                # y_table = y[condition]
                # z_table = z[condition]

                # fig = plt.figure()  
                # ax = fig.add_subplot(111, projection='3d')  
                # ax.scatter(x_table, y_table, z_table)  
                # ax.set_xlabel('X')  
                # ax.set_ylabel('Y')  
                # ax.set_zlabel('Z')
                # plt.show()
                # exit()

                # x3 = np.array(x)
                # y3 = np.array(y)
                # z3 = np.array(z)

                # a_degrees = -90.0
                # a_radians = radians(a_degrees)
                # points = np.vstack((x3, y3, z3))
                # cos_a = np.cos(a_radians)
                # sin_a = np.sin(a_radians)
                # rotation_matrix = np.array([
                #     [cos_a, 0, -sin_a],
                #     [0, 1, 0],
                #     [sin_a, 0, cos_a]
                # ])
                # rotated_points = np.dot(rotation_matrix, points)
                # x = rotated_points[0]
                # y = rotated_points[1]
                # z = rotated_points[2]

                # 操作前的极值和数据范围
                xmax = x.max()
                xmin = x.min()
                ymax = y.max()
                ymin = y.min()
                zmax = z.max()
                zmin = z.min()

                x_range = xmax - xmin
                y_range = ymax - ymin
                z_range = zmax - zmin

                # 去除设定范围以外点
                # condition = (x > -0.5 * x_length) & (x < 0.5 * x_length) & (z > (-z_length)) & (z < 0) & (y > ymin + 0.5 * (y_range - y_length)) & (y < ymax - 0.5 * (y_range - y_length))
                # condition = (x > -0.5 * x_length) & (x < 0.5 * x_length) & (z > zmin + 0.5 * (z_range - z_length)) & (z < zmax - 0.5 * (z_range - z_length)) & (y > ymin + 0.5 * (y_range - y_length)) & (y < ymax - 0.5 * (y_range - y_length))
                # condition = (x > 1500) & (x < 3000) & (y > 3000) & (y < 4000) & (z > (-z_length)) & (z < 0)
                # condition = (x > -0.5 * x_length) & (z > (-z_length)) & (z < 0) & (y > ymin + 0.5 * (y_range - y_length)) & (y < ymax - 0.5 * (y_range - y_length))
                # condition = (x > -4000) & (z > (-z_length)) & (z < 0) & (y > ymin + 0.5 * (y_range - y_length)) & (x < 4200)
                condition = (x > -0.5 * x_length) & (x < 0.5 * x_length) & (z > (-z_length)) & (z < 0) & (y > ymin + 0.5 * (y_range - y_length)) & (y < ymax - 0.5 * (y_range - y_length))
                x = x[condition]
                y = y[condition]
                z = z[condition]
                print(len(x))

                # 操作后的极值和数据范围
                xmax = x.max()
                xmin = x.min()
                ymax = y.max()
                ymin = y.min()
                zmax = z.max()
                zmin = z.min()

                x_range = xmax - xmin
                y_range = ymax - ymin
                z_range = zmax - zmin
                '''
                fig = plt.figure(figsize=(25.6, 14.4))

                peo_h, height_x, height_y, height_z = height(x, y, z)

                ax2 = fig.add_subplot(5, 1, (1, 4), projection='3d')
                ax2.set_box_aspect([x_range, y_range, z_range])

                # 设置三维图图形区域背景颜色（r,g,b,a）
                ax2.xaxis.set_pane_color((0.0, 0.0, 0.0, 1.0))
                ax2.yaxis.set_pane_color((0.0, 0.0, 0.0, 1.0))
                ax2.zaxis.set_pane_color((0.0, 0.0, 0.0, 1.0))

                # 修改背景颜色
                ax2.set_facecolor("black")
                ax2.patch.set_alpha(0.0)

                # 坐标点颜色
                ax2.scatter(x, y, z, s=0.05, c=z, cmap=cmap_kinds)
                ax2.view_init(elev=30, azim=285)

                # "viridis": 这是Matplotlib的默认颜色映射，适合于连续的数据。颜
                # "viridis": 这是Matplotlib的默认颜色映射，适合于连续的数据。颜色从深蓝色到亮黄色过渡。
                # "plasma": 类似于"viridis"，但颜色更丰富，从深蓝到亮紫过渡。
                # "inferno": 从深蓝到红色过渡，适合强调高值。
                # "magma": 类似于"inferno"，但更适合黑底背景。
                # "cividis": 对色盲友好的颜色映射，从深蓝到亮黄过渡。

                # setting title and labels
                # if areas <= 1100000:
                #     val = "偏瘦"
                # if areas >= 2200000:
                #     val = "偏胖"
                # if 1100000 < areas < 2200000:
                #     val = "标准"
                if int(peo_h) <= 70:
                    ax2.set_title("3D扫描图\n扫描身高约为：无效数据\n人体表面积约为：无效数据\n身体情况：无法判断")
                if int(peo_h) > 70:
                    ax2.set_title("3D扫描图\n扫描身高约为：" + str(peo_h) + " 厘米\n身体情况：标准")
                ax2.set_xlim([xmin, xmax])
                ax2.set_ylim([ymin, ymax])
                ax2.set_zlim([zmin, zmax])
                ax2.set_xlabel('x轴(毫米)')
                ax2.set_ylabel('y轴(毫米)')
                ax2.set_zlabel('z轴(毫米)')
                '''
                # 出2图
                # ax1 = fig.add_subplot(337)
                ax1 = plt.figure()
                ax1 = ax1.add_subplot(111)   
                ax1.set_box_aspect(y_range / x_range)

                ax1.scatter(x, y, s=0.05, c=z, cmap=cmap_kinds)
                # setting title and labels
                ax1.set_title("俯视图")
                ax1.set_xlabel('x轴(毫米)')
                ax1.set_ylabel('y轴(毫米)')
                ax1.set_xlim([xmin, xmax])
                ax1.set_ylim([ymin, ymax])
                # plt.xticks(np.arange(-4000, 5000, 250))  # 设置x轴刻度间距为0.5  
                plt.yticks(np.arange(0, 5000, 250))  # 设置y轴刻度间距为0.1 

                # 修改背景颜色
                ax1.patch.set_facecolor('black')
                ax1.grid()
                '''
                # 出3图
                ax1 = fig.add_subplot(338)
                ax1.set_box_aspect(z_range / x_range)
                ax1.scatter(x, z, s=0.05, c=z, cmap=cmap_kinds)
                # setting title and labels
                ax1.set_title("正视图  扫描身高为:" + str(peo_h) + "厘米")
                ax1.set_xlabel('x轴(毫米)')
                ax1.set_ylabel('z轴(毫米)')
                ax1.set_xlim([xmin, xmax])
                ax1.set_ylim([zmin, zmax])

                # 修改背景颜色
                ax1.patch.set_facecolor('black')
                ax1.grid()

                # 出4图
                ax1 = fig.add_subplot(339)
                ax1.set_box_aspect(z_range / y_range)
                ax1.scatter(y, z, s=0.05, c=z, cmap=cmap_kinds)
                # setting title and labels
                ax1.set_title("侧视图")
                ax1.set_xlabel('y轴(毫米)')
                ax1.set_ylabel('z轴(毫米)')
                ax1.set_xlim([ymin, ymax])
                ax1.set_ylim([zmin, zmax])

                # 修改背景颜色
                ax1.patch.set_facecolor('black')
                ax1.grid()
'''
                # 保存图像
                if saveFig == 1:
                    plt.savefig(r'.\data\box\%s_suofang.png' % filename)
                #     saveFig = 0

                filesize = 0

    print("数据全部读取完成，即将生成扫描图，请耐心等待......")
    time.sleep(3)

work()
plt.show()
print("出图成功，请查看")